FAST-LIO2 详解：在校园智巡项目中的角色
文档版本: 2026-06-21
运行平台: Intel N100 Mini PC (Ubuntu 22.04)
前置知识: 基本的 ROS2 概念、了解 EKF 传感器融合的基本原理
1. FAST-LIO2 是什么？
FAST-LIO2（Fast LiDAR-Inertial Odometry 2）是香港大学火星实验室（MARS Lab）开发的一个LiDAR 惯性里程计系统。它的核心能力是：给定一个 3D LiDAR 和一个 IMU，实时输出机器人精确的 6DOF（六自由度）位姿估计。
"里程计"在这里的含义是：它告诉你机器人相对于启动位置移动了多少、转了多少。就像车上的里程表，但精度高得多——位置精度约 1cm，yaw 角度精度约 0.1°。
它不是 SLAM（虽然看起来类似）：
- SLAM 的目标是建图 + 定位，输出一张可复用的全局地图
- FAST-LIO2 的目标是只做里程计，它内部维护的地图只用于帧间匹配，不输出给外部使用
- 在我们的项目中，全局定位由 RTK-GPS 负责，FAST-LIO2 只负责高频高精度的相对运动估计
2. 为什么是 FAST-LIO2 而不是其他方案？
2.1 与轮式里程计对比
轮式里程计通过轮子转数推算运动，核心问题：
- 轮子打滑 → 位置误差
- 轮径/轮距不精确 → yaw 积分漂移
- 没有环境感知能力 → 无法检测和修正漂移
- Yahboomcar 底盘速度不确定度约 ±5cm/s，对于最大速度 26cm/s 误差率约 20%
FAST-LIO2 通过环境几何特征匹配来估计运动，精度高一到两个数量级，且不受轮子打滑影响。
2.2 与其他 LiDAR 里程计方案对比
| 方案 | 优点 | 缺点 | 适合本项目？ |
|---|---|---|---|
| LOAM / A-LOAM | 经典方案 | 不支持 Livox 非重复扫描 | ❌ |
| LIO-SAM | GPS 融合方便 | 不原生支持 Livox | ❌ |
| KISS-ICP | 极简，只需点云，不需要 IMU | 激烈运动时不稳定 | ⚠️ 备选 |
| Point-LIO | 支持 Livox，点级注册 | 计算量大 | ⚠️ |
| FAST-LIO2 | 专为 Livox 设计，效率极高，IMU 紧耦合 | 需要 IMU 频率较高 | ✅ 首选 |
FAST-LIO2 被选中的核心原因：
- 专门支持 Livox 的非重复扫描模式
- ikd-Tree 数据结构使得地图更新极快，N100 轻松跑得动
- IMU 紧耦合提供高频位姿预测，点云匹配更稳定
- x86 平台编译无 ARM 兼容问题，依赖链清晰
备选方案：如果底盘的 10Hz IMU 不满足 FAST-LIO2 的要求，KISS-ICP 是备选——它完全不需要 IMU，只用点云做帧间匹配，但精度和鲁棒性略低于 FAST-LIO2。
3. FAST-LIO2 内部原理
3.1 总体管线
每当一帧新的 LiDAR 点云到达，FAST-LIO2 执行以下步骤：
新点云帧到达
│
▼
[1] IMU 前向传播（预测）
│ 利用两帧点云之间的所有 IMU 数据
│ 通过加速度/角速度积分预测当前位姿
│ 这给出一个粗略但高频的位姿估计
│
▼
[2] 点云去畸变
│ LiDAR 扫描不是瞬间完成的（Livox Mid-360 一帧约 100ms）
│ 在这 100ms 内机器人在移动，所以每个点被采集时的位姿不同
│ 利用 IMU 预测的连续位姿轨迹，把每个点变换到帧结束时刻的坐标系
│ 这消除了运动畸变
│
▼
[3] 点云降采样
│ 对去畸变后的点云做体素滤波
│ 减少点数，降低后续计算量
│ 典型参数：体素大小 0.3-0.5m
│
▼
[4] Iterated EKF 更新（核心）
│ 这是 FAST-LIO2 的精华
│ 把每个降采样后的点与局部地图（ikd-Tree）做最近邻匹配
│ 构造点-到-面残差（point-to-plane residual）
│ 通过 iterated EKF 迭代优化当前位姿
│ 通常 2-3 次迭代就收敛
│
▼
[5] 地图更新
│ 把当前帧的点云（用优化后的位姿变换到世界坐标系）
│ 插入 ikd-Tree
│ 删除距离太远的旧点（维持局部地图大小）
│
▼
输出：优化后的 6DOF 位姿 → 发布为 Odometry 消息3.2 IMU 紧耦合
"紧耦合"的意思是 IMU 不是作为外部输入被简单加权融合，而是在数学模型内部与 LiDAR 观测联合优化。
具体来说：
- 状态向量包含：位置 (3)、速度 (3)、姿态四元数 (4)、IMU 加速度零偏 (3)、IMU 陀螺仪零偏 (3) = 共 16 维
- 预测步：用 IMU 数据做运动预测（高频，理想 100-400Hz）
- 更新步：用 LiDAR 点-到-面残差做观测更新（10Hz，每帧点云到达时）
- 零偏在线估计：IMU 的加速度计零偏和陀螺仪零偏作为状态变量被持续估计和修正
这意味着即使底盘 IMU 质量不高（零偏大），FAST-LIO2 也会自动估计并补偿这个零偏。
关于 IMU 频率：理想状态下 IMU 应该 100Hz 以上。底盘的 10Hz IMU 意味着两帧点云之间（100ms）只有 1 个 IMU 样本，运动补偿精度受限。这是一个需要实测验证的风险点——如果效果不佳，可以外接一个独立的高频 IMU（如 BNO055 USB 100Hz），或者退而使用不需要 IMU 的 KISS-ICP。
3.3 ikd-Tree（增量式 KD 树）
传统 KD 树在每次插入新点后需要重建，计算成本高。ikd-Tree 是 FAST-LIO2 团队提出的增量式 KD 树，支持：
- 增量插入：新点直接插入现有树，不重建
- 增量删除：通过"懒删除"标记过期点，定期整理
- 范围删除：一次性删除某个空间区域内的所有点（用于维护局部地图大小）
- 并行重平衡：在后台线程重平衡树结构，不阻塞主线程
这使得地图更新的计算量极低——每帧只需几毫秒，即使地图包含数十万个点。在 N100 的 x86 处理器上，ikd-Tree 的性能比 ARM 平台更好（更高的单核 IPC + 更好的分支预测）。
3.4 Livox 非重复扫描的特殊处理
传统机械旋转 LiDAR（如 Velodyne）每次扫描覆盖相同的角度。但 Livox Mid-360 使用非重复扫描模式——每一帧的扫描线都不同，随时间积累逐渐覆盖整个 FoV。
这对传统 LiDAR 里程计是个问题，因为它们假设每帧的扫描模式相同。但 FAST-LIO2 不依赖扫描线结构——它对每个点独立做点-到-面匹配，所以天然兼容 Livox 的非重复扫描。
而且 Livox 的非重复扫描实际上是一个优势：随着时间推移，ikd-Tree 中的局部地图会自动积累越来越密集、覆盖越来越完整的点云，使得匹配越来越精确。
4. FAST-LIO2 在本项目中的具体角色
4.1 替代轮式里程计
在 v1 中，EKF 的 odom0 是轮式里程计（通过 covariance_relay.py 注入协方差后的 /odom/relay）。在 v2 中，odom0 改为 FAST-LIO2 的输出 /lio_odom。
精度对比：
| 指标 | 轮式里程计 | FAST-LIO2 |
|---|---|---|
| 位置增量精度 | ±5cm/s | ±0.5cm/s |
| yaw 角速度精度 | ±5°/s (drift) | ±0.1°/s |
| 10秒内 yaw 累积误差 | 可达 5-10° | < 0.5° |
| 环境适应性 | 与地面摩擦相关 | 与环境几何特征相关 |
| 失效场景 | 打滑、粗糙路面 | 极空旷无特征环境 |
4.2 提供 GPS 空窗期的连续定位
v1 中，如果 GPS 从 RTK Fixed 降级到 SPP（精度 ±5m），EKF 的位姿估计会剧烈抖动。v2 中，即使 GPS 完全丢失：
- 前 10 秒：FAST-LIO2 漂移 < 5cm，导航完全不受影响
- 前 1 分钟：漂移 < 30cm，仍可正常避障和跟踪路径
- 前 5 分钟：漂移 ~1-2m，导航可能偏离目标但不会失控
- GPS 恢复后：EKF 利用 GPS 绝对位置自动修正累积漂移
4.3 改善 DWB 控制器的跟踪质量
DWB 控制器每 100ms 读取 EKF 输出的位姿。v1 中 EKF 只能 5Hz 输出。v2 中 FAST-LIO2 输出 10Hz + N100 算力充足，EKF 可以稳定 10Hz 甚至更高。
更重要的是，FAST-LIO2 提供的速度和角速度估计比轮式里程计精确得多。DWB 看到的"我正在以 0.15m/s 向前走、以 0.02rad/s 旋转"终于是准确的了。
5. FAST-LIO2 的输入输出
5.1 输入
| 话题 | 类型 | 来源 | 频率 | 说明 |
|---|---|---|---|---|
/livox/lidar | PointCloud2 | Livox Mid-360 | 10Hz | 3D 点云 |
/imu/data_raw | Imu | 底盘 IMU | 10Hz（理想100Hz+） | 加速度 + 角速度 |
5.2 输出
| 话题 | 类型 | 频率 | 内容 |
|---|---|---|---|
/lio_odom | Odometry | 10Hz | 6DOF 位姿 + 速度 + 协方差 |
/cloud_registered | PointCloud2 | 10Hz | 配准后的点云（可选，调试用） |
5.3 关键配置参数
# ===== FAST-LIO2 配置文件 =====
# LiDAR 类型
lidar_type: 1 # 1 = Livox 自定义格式
scan_line: 4 # Mid-360 的扫描线数
blind: 0.5 # 忽略距离传感器太近的点（米）
# 点云降采样
filter_size_surf: 0.3 # 表面点体素大小（米），越小越精确但越慢
filter_size_map: 0.3 # 地图体素大小
# IMU 参数（需要根据实际 IMU 调整）
acc_cov: 0.1 # 加速度计噪声协方差
gyr_cov: 0.1 # 陀螺仪噪声协方差
b_acc_cov: 0.0001 # 加速度计零偏随机游走
b_gyr_cov: 0.0001 # 陀螺仪零偏随机游走
# LiDAR-IMU 外参（需要准确测量或标定）
extrinsic_T: [0.14, 0.0, 0.12] # IMU→LiDAR 的平移 [x, y, z]
extrinsic_R: [1,0,0, 0,1,0, 0,0,1] # IMU→LiDAR 的旋转矩阵（需确认）
# 地图管理
cube_side_length: 200 # 局部地图范围（米），200m 足够园区场景
max_iteration: 3 # iterated EKF 最大迭代次数
# 输出配置
publish_odometry: true # 发布 Odometry 消息
publish_tf: false # 不发布 TF（由外部 EKF 统一管理）5.4 LiDAR-IMU 外参标定
在我们的平台上：
- LiDAR (Livox Mid-360) 位于 base_link 前方 0.14m，高 0.12m
- IMU 内置在底盘中心（约 base_link 原点）
- IMU→LiDAR 平移约
[0.14, 0.0, 0.12] - 旋转取决于两者坐标系定义，初始设为单位矩阵，实测后调整
如果外参不准确：直线行驶时 yaw 会缓慢漂移，转弯时位置估计出现系统性偏差。可以通过让机器人走直线观察轨迹是否弯曲来快速判断外参是否有问题。
6. FAST-LIO2 的局限性
6.1 无法替代 GPS 的绝对定位
FAST-LIO2 提供相对里程计——知道从启动位置出发移动了多少，但不知道地球上的绝对位置。长期运行后有累积漂移（虽然远小于轮式里程计）。所以它与 GPS 互补，不能替代。
6.2 对环境几何特征的依赖
如果环境极其空旷（如空旷操场中心，50m 内无建筑物），点云匹配约束不足，精度下降。在校园环境中（建筑物、树木、围栏、路灯），这个问题不大。
6.3 IMU 频率要求
理想 100Hz+。底盘 10Hz IMU 是一个风险点，需要实测。
6.4 计算资源
在 N100（x86 4核）上预计占用约 15-25% CPU。比在 Xavier NX（ARM 6核）上更高效，因为 x86 的单核 IPC 更高、分支预测更好、编译优化更成熟。加上 Nav2 全栈，N100 应该有足够余量。
7. 与项目中其他组件的关系
Livox Mid-360 ─┬──→ FAST-LIO2 ──→ /lio_odom ──→ EKF ──→ Nav2
│ ↑
│ /imu/data_raw (底盘 IMU)
│
└──→ STVL ──→ costmap ──→ Nav2
UM982 GPS ──→ navsat_transform ──→ /odometry/gps ──→ EKFFAST-LIO2 不与 STVL 交互，不与 GPS 交互，不与 Nav2 交互。它只做一件事：把 LiDAR + IMU 数据变成高精度的里程计输出。这种解耦意味着出了问题可以独立排查和替换。
8. 参考资料
- 论文: Xu, W., & Zhang, F. (2022). FAST-LIO2: Fast Direct LiDAR-Inertial Odometry. IEEE T-RO.
- 原始仓库 (ROS1): https://github.com/hku-mars/FAST_LIO
- ROS2 移植: 社区有多个版本，选择 Humble + Livox 兼容的版本
- ikd-Tree 论文: Cai, Y., et al. (2021). ikd-Tree: An Incremental KD Tree for Robotic Applications.